#pragma config(Hubs,  S1, HTMotor,  HTMotor,  HTServo,  none)
#pragma config(Hubs,  S4, HTMotor,  HTMotor,  none,     none)
#pragma config(Sensor, S3,     sonar,          sensorSONAR)
#pragma config(Motor,  mtr_S1_C1_1,     frontLeft,     tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     rearLeft,      tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C2_1,     sweeper,       tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S1_C2_2,     arm,           tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C1_1,     rearRight,     tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S4_C1_2,     frontRight,    tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S4_C2_1,     lift,          tmotorTetrix, openLoop)
#pragma config(Motor,  mtr_S4_C2_2,     motorK,        tmotorTetrix, openLoop)
#pragma config(Servo,  srvo_S1_C3_1,    guiderDrop,           tServoStandard)
#pragma config(Servo,  srvo_S1_C3_2,    leftHook,             tServoStandard)
#pragma config(Servo,  srvo_S1_C3_3,    rightHook,            tServoStandard)
#pragma config(Servo,  srvo_S1_C3_4,    pivot,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_5,    doors,                tServoStandard)
#pragma config(Servo,  srvo_S1_C3_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include <JoystickDriver.c>
#include "Cascade_Functions.h"

task main()
{
	phase = AUTO;
	driveCycles = 6;
	humor = 120;
	bDisplayDiagnostics = false;
	initializeAutonomous();
	bDisplayDiagnostics = true;
	waitForStart();
	motor [lift] = 100;
	wait1Msec(750);
	motor [lift] = 0;
	wait1Msec(250);
	//#if 0
	while (nMotorEncoder [frontLeft] > -(driveCycles * driveEncoderCycle))
	{
		drive(0, -25, 0);
	}
	while (SensorValue [sonar] > 42)
	{
		drive(0, 0, 25);
	}
	wait1Msec(350);
	allStop();
	//StartTask(failSafe);
	ClearTimer(T4);
	while (SensorValue [sonar] > 20)
	{
		drive(0, -20, 0);
		if (time1[T4] >= 2000)
		{
			allStop();
			while (SensorValue [sonar] > 28)
			{
				drive(0, 0, -40);
			}
			drive(0, -20, 0);
			wait1Msec(1000);
			break;
		}
	}
	//StopTask(failSafe);
	//#endif
	wait1Msec(500);
	allStop();
	servo [leftHook] = 168;
	servo [rightHook] = 16;
	wait1Msec(100);
	score(60);

	servo [doors] = 122;
	wait1Msec(250);
	servo [doors] = 140;
	wait1Msec(250);
	servo [doors] = 150;
	wait1Msec(250);
	servo [doors] = 160;
	wait1Msec(250);
	servo [doors] = 122;
	wait1Msec(50);
	//#if 0
	drive(0, 0, -90);
	wait1Msec(1000);
	allStop();
	nMotorEncoder [frontLeft] = 0;
	while (nMotorEncoder [frontLeft] > -0.8 * driveEncoderCycle)
	{
		drive(0, -75, 0);
	}
	allStop();
	servo [leftHook] = 35;
	servo [rightHook] = 140;
	wait1Msec(250);
	while (nMotorEncoder [frontLeft] < -0.4 * driveEncoderCycle)
	{
		drive(0, 75, 0);
	}
	allStop();
	//drive(0, 0, 50);
	//wait1Msec(1500);
	//drive(0, -50, 0);
	//wait1Msec(450);
	while (SensorValue [sonar] > 58)
	{
		drive(0, 0, 25);
	}
	wait1Msec(550);
	allStop();
	//ClearTimer(T3);
	//motor [lift] = 100;
	//while (time1[T3] < 1500)
	//{
	//}
	//motor [lift] = 0;
	StartTask(failSafe);
	while (SensorValue [sonar] > 38)
	{
		drive(0, -20, 0);
	}
	StopTask(failSafe);
	score(90);
	StartTask(failSafe);
	while (SensorValue [sonar] > 20)
	{
		drive(0, -20, 0);
	}
	allStop();
	StopTask(failSafe);
	wait1Msec(250);
	servo [leftHook] = 168;
	servo [rightHook] = 16;

	servo [doors] = 122;
	wait1Msec(200);
	servo [doors] = 90;
	wait1Msec(200);
	servo [doors] = 69;
	wait1Msec(200);
	servo [doors] = 30;
	wait1Msec(200);
	servo [doors] = 0;
	wait1Msec(250);
	servo [doors] = 122;
	wait1Msec(75);
	drive(0, 100, 0);
	motor [arm] = 100;
	servo [pivot] = 245;
	wait1Msec(750);
	allStop();
	wait1Msec(250);
	motor [arm] = 0;
	//#endif
}
